const DATA_SIZE: usize = 104;
const FRAME_SIZE: usize = 1 + 1 + 2 + 2 + DATA_SIZE;
const HEADER: u8 = 0x5a;

enum ParseState {
    WaitHeader,
    WaitType,
    CollectDataLen,
    CollectCRC,
    Collectdata,
}

pub struct FrameParser {
    state: ParseState,
    frame: [u8; FRAME_SIZE],
    idx: usize,
    crc: u16,
}

impl Default for FrameParser {
    fn default() -> Self {
        Self::new()
    }
}

impl FrameParser {
    pub fn new() -> Self {
        Self {
            state: ParseState::WaitHeader,
            frame: [0; FRAME_SIZE],
            idx: 0,
            crc: 0,
        }
    }

    pub fn parse_byte(&mut self, x: u8) -> Option<&[u8]> {
        match self.state {
            ParseState::WaitHeader => {
                if x == HEADER {
                    self.frame[self.idx] = x;
                    self.idx += 1;

                    self.crc = crc16_update_byte(self.crc, x);

                    self.state = ParseState::WaitType;
                }
                None
            },
            ParseState::WaitType => {
                self.frame[self.idx] = x;
                self.idx += 1;

                self.crc = crc16_update_byte(self.crc, x);

                self.state = ParseState::CollectDataLen;

                None
            },
            ParseState::CollectDataLen => {
                self.frame[self.idx] = x;
                self.idx += 1;

                self.crc = crc16_update_byte(self.crc, x);

                if self.idx >= 4 {
                    self.state = ParseState::CollectCRC;
                }

                None
            },
            ParseState::CollectCRC => {
                self.frame[self.idx] = x;
                self.idx += 1;

                if self.idx >= 6 {
                    self.state = ParseState::Collectdata;
                }

                None
            },
            ParseState::Collectdata => {
                self.frame[self.idx] = x;
                self.idx += 1;

                self.crc = crc16_update_byte(self.crc, x);

                if self.idx >= FRAME_SIZE {
                    let crc = self.crc;
                    self.reset_state();

                    if crc == self.frame[4] as u16 | ((self.frame[5] as u16) << 8) {
                        return Some(&self.frame);
                    }

                    return None;
                }

                None
            },
        }
    }

    pub fn parse_buffer<T: FnMut(&[u8])>(&mut self, buffer: &[u8], mut callback: T) {
        for &b in buffer {
            if let Some(fr) = self.parse_byte(b) {
                callback(fr);
            }
        }
    }

    fn reset_state(&mut self) {
        self.state = ParseState::WaitHeader;
        self.idx = 0;
        self.crc = 0;
    }
}


fn crc16_update_byte(crc: u16, byte: u8) -> u16 {
    let mut crc = crc;
    crc ^= (byte as u16) << 8;
    
    for _ in 0..8 {
        let mut temp = crc << 1;
        if (crc & 0x8000) != 0 {
            temp ^= 0x1021;
        }
        crc = temp;
    }
    
    crc
}